/**************************************************************
 * obstacle_avoidance.h
 * 
 * 实时避障算法头文件
 * 
 *  Created on: 2024-01-20
 *      Author: robot
 *************************************************************/

#ifndef OBSTACLE_AVOIDANCE_H
#define OBSTACLE_AVOIDANCE_H

#include <stdint.h>
#include <stdbool.h>
#include "lidar_ld06.h"

// 避障动作定义
typedef enum {
    ACTION_MOVE_FORWARD = 0,
    ACTION_SLIGHT_LEFT,
    ACTION_SLIGHT_RIGHT, 
    ACTION_TURN_LEFT,
    ACTION_TURN_RIGHT,
    ACTION_EMERGENCY_STOP
} ObstacleAction;

// 障碍物方向定义
typedef enum {
    DIRECTION_FRONT = 0,
    DIRECTION_LEFT,
    DIRECTION_RIGHT,
    DIRECTION_COUNT
} ObstacleDirection;

// 前方避障滤波器
typedef struct {
    uint16_t front_distance_history[5];
    uint8_t history_index;
    uint8_t valid_count;
    uint16_t current_safe_distance;
} ObstacleAvoidanceFilter;

// 多方向障碍物检测结果
typedef struct {
    uint16_t min_distances[DIRECTION_COUNT];
    uint8_t obstacle_flags[DIRECTION_COUNT];
} MultiDirectionObstacle;

// 完整避障系统
typedef struct {
    ObstacleAvoidanceFilter front_filter;
    MultiDirectionObstacle current_obstacles;
    uint32_t last_update_time;
    uint16_t emergency_stop_distance;
    uint16_t safe_distance;
} ObstacleAvoidanceSystem;

// 函数声明
void InitObstacleSystem(ObstacleAvoidanceSystem *system, uint16_t emergency_distance, uint16_t safe_distance);
ObstacleAction ProcessObstacleAvoidance(ObstacleAvoidanceSystem *system, LiDARFrameTypeDef *frame);
uint16_t GetFrontObstacleDistance(ObstacleAvoidanceSystem *system);
void GetMultiDirectionDistances(ObstacleAvoidanceSystem *system, uint16_t *front, uint16_t *left, uint16_t *right);

#endif /* OBSTACLE_AVOIDANCE_H */